/*
 * File: Auto.cpp
 *
 * Code generated for Simulink model 'Auto'.
 *
 * Model version                  : 1.259
 * Simulink Coder version         : 8.0 (R2011a) 09-Mar-2011
 * TLC version                    : 8.0 (Feb  3 2011)
 * C/C++ source code generated on : Tue Sep 20 20:17:44 2011
 *
 * Target selection: ert.tlc
 * Embedded hardware selection: Texas Instruments->C2000
 * Code generation objectives: Unspecified
 * Validation result: Not run
 */

#include "Auto.h"
#include "Auto_private.h"

/* Named constants for Stateflow: '<S1>/Chart' */
#define Auto_IN_Auto                   (1U)
#define Auto_IN_Off                    (2U)
#define Auto_IN_TakeOff                (3U)

/* Block signals (auto storage) */
BlockIO_Auto Auto_B;

/* Block states (auto storage) */
D_Work_Auto Auto_DWork;

/* External inputs (root inport signals with auto storage) */
ExternalInputs_Auto Auto_U;

/* External outputs (root outports fed by signals with auto storage) */
ExternalOutputs_Auto Auto_Y;

/* Real-time model */
RT_MODEL_Auto Auto_M_;
RT_MODEL_Auto *const Auto_M = &Auto_M_;

/* Model step function */
void Auto_step(void)
{
  real_T rtb_Thr;
  real_T rtb_edRoll;
  real_T rtb_FilterCoefficient;
  real_T rtb_edPitch;
  real_T rtb_FilterCoefficient_i;
  real_T rtb_Saturation_a;
  real_T rtb_FilterCoefficient_j;
  real_T rtb_edYaw;
  real_T rtb_FilterCoefficient_f;
  real_T rtb_FilterCoefficient_a;
  real_T u;

  /* Gain: '<S4>/Gain' incorporates:
   *  Bias: '<S4>/Bias'
   *  Inport: '<Root>/Thr'
   */
  rtb_Thr = (Auto_U.Thr + Auto_P.Bias_Bias_jz) * Auto_P.Gain_Gain_m;

  /* Stateflow: '<S1>/Chart' */

  /* Gateway: Auto Ctrl/Chart */
  /* During: Auto Ctrl/Chart */
  if ((int16_T)Auto_DWork.is_active_c1_Auto == 0) {
    /* Entry: Auto Ctrl/Chart */
    Auto_DWork.is_active_c1_Auto = 1U;

    /* Transition: '<S3>:5' */
    Auto_DWork.is_c1_Auto = Auto_IN_Off;

    /* Entry 'Off': '<S3>:1' */
    Auto_B.EnThr = 0.0;
  } else {
    switch (Auto_DWork.is_c1_Auto) {
     case Auto_IN_Auto:
      /* During 'Auto': '<S3>:4' */
      if (rtb_Thr < 1050.0) {
        /* Transition: '<S3>:12' */
        Auto_B.Auto_d = FALSE;
        Auto_DWork.is_c1_Auto = Auto_IN_Off;

        /* Entry 'Off': '<S3>:1' */
        Auto_B.EnThr = 0.0;
      }
      break;

     case Auto_IN_Off:
      /* During 'Off': '<S3>:1' */
      if (rtb_Thr > 1050.0) {
        /* Transition: '<S3>:6' */
        Auto_DWork.is_c1_Auto = Auto_IN_TakeOff;

        /* Entry 'TakeOff': '<S3>:3' */
        Auto_B.EnThr = 1.0;
      }
      break;

     case Auto_IN_TakeOff:
      /* During 'TakeOff': '<S3>:3' */
      if (rtb_Thr > 1150.0) {
        /* Transition: '<S3>:8' */
        Auto_DWork.is_c1_Auto = Auto_IN_Auto;
        Auto_B.Auto_d = TRUE;

        /* Entry 'Auto': '<S3>:4' */
        Auto_B.EnThr = 0.0;
      } else {
        if (rtb_Thr < 1050.0) {
          /* Transition: '<S3>:11' */
          Auto_DWork.is_c1_Auto = Auto_IN_Off;

          /* Entry 'Off': '<S3>:1' */
          Auto_B.EnThr = 0.0;
        }
      }
      break;

     default:
      /* Transition: '<S3>:5' */
      Auto_DWork.is_c1_Auto = Auto_IN_Off;

      /* Entry 'Off': '<S3>:1' */
      Auto_B.EnThr = 0.0;
      break;
    }
  }

  /* End of Stateflow: '<S1>/Chart' */

  /* Outputs for Enabled SubSystem: '<S1>/AutoController' incorporates:
   *  EnablePort: '<S2>/Enable'
   */
  if (Auto_B.Auto_d) {
    if (!Auto_DWork.AutoController_MODE) {
      /* InitializeConditions for DiscreteIntegrator: '<S18>/Filter' */
      Auto_DWork.Filter_DSTATE = Auto_P.Filter_IC;

      /* InitializeConditions for DiscreteIntegrator: '<S19>/Integrator' */
      Auto_DWork.Integrator_DSTATE = Auto_P.Integrator_IC;

      /* InitializeConditions for DiscreteIntegrator: '<S19>/Filter' */
      Auto_DWork.Filter_DSTATE_i = Auto_P.Filter_IC_g;

      /* InitializeConditions for DiscreteIntegrator: '<S13>/Integrator' */
      Auto_DWork.Integrator_DSTATE_a = Auto_P.Integrator_IC_d;

      /* InitializeConditions for DiscreteIntegrator: '<S13>/Filter' */
      Auto_DWork.Filter_DSTATE_c = Auto_P.Filter_IC_d;

      /* InitializeConditions for DiscreteIntegrator: '<S12>/Integrator' */
      Auto_DWork.Integrator_DSTATE_j = Auto_P.Integrator_IC_g;

      /* InitializeConditions for DiscreteIntegrator: '<S12>/Filter' */
      Auto_DWork.Filter_DSTATE_m = Auto_P.Filter_IC_c;

      /* InitializeConditions for DiscreteIntegrator: '<S14>/Filter' */
      Auto_DWork.Filter_DSTATE_a = Auto_P.Filter_IC_o;

      /* InitializeConditions for DiscreteIntegrator: '<S14>/Integrator' */
      Auto_DWork.Integrator_DSTATE_m = Auto_P.Integrator_IC_c;
      Auto_DWork.AutoController_MODE = TRUE;
    }

    /* Bias: '<S2>/Bias' incorporates:
     *  Gain: '<S2>/Gain'
     */
    rtb_Saturation_a = Auto_P.Gain_Gain * rtb_Thr + Auto_P.Bias_Bias;

    /* Sum: '<S16>/Sum2' incorporates:
     *  Gain: '<S16>/JoyScale'
     *  Inport: '<Root>/RPY'
     *  Inport: '<Root>/Roll'
     */
    rtb_edRoll = Auto_P.JoyScale_Gain * Auto_U.Roll - Auto_U.RPY[0];

    /* Gain: '<S18>/Filter Coefficient' incorporates:
     *  DiscreteIntegrator: '<S18>/Filter'
     *  Gain: '<S18>/Derivative Gain'
     *  Sum: '<S18>/SumD'
     */
    rtb_FilterCoefficient = (Auto_P.DerivativeGain_Gain * rtb_edRoll -
      Auto_DWork.Filter_DSTATE) * Auto_P.FilterCoefficient_Gain;

    /* Saturate: '<S18>/Saturation' incorporates:
     *  Gain: '<S18>/Proportional Gain'
     *  Sum: '<S18>/Sum'
     */
    u = Auto_P.ProportionalGain_Gain * rtb_edRoll + rtb_FilterCoefficient;

    /* Sum: '<S16>/Sum1' incorporates:
     *  Inport: '<Root>/omega'
     *  Saturate: '<S18>/Saturation'
     */
    rtb_edRoll = (u >= Auto_P.Saturation_UpperSat ? Auto_P.Saturation_UpperSat :
                  u <= Auto_P.Saturation_LowerSat ? Auto_P.Saturation_LowerSat :
                  u) - Auto_U.omega[0];

    /* Gain: '<S19>/Filter Coefficient' incorporates:
     *  DiscreteIntegrator: '<S19>/Filter'
     *  Gain: '<S19>/Derivative Gain'
     *  Sum: '<S19>/SumD'
     */
    rtb_FilterCoefficient_i = (Auto_P.DerivativeGain_Gain_a * rtb_edRoll -
      Auto_DWork.Filter_DSTATE_i) * Auto_P.FilterCoefficient_Gain_o;

    /* Saturate: '<S19>/Saturation' incorporates:
     *  DiscreteIntegrator: '<S19>/Integrator'
     *  Gain: '<S19>/Proportional Gain'
     *  Sum: '<S19>/Sum'
     */
    u = (Auto_P.ProportionalGain_Gain_n * rtb_edRoll +
         Auto_DWork.Integrator_DSTATE) + rtb_FilterCoefficient_i;
    rtb_edPitch = u >= Auto_P.Saturation_UpperSat_p ?
      Auto_P.Saturation_UpperSat_p : u <= Auto_P.Saturation_LowerSat_h ?
      Auto_P.Saturation_LowerSat_h : u;

    /* Saturate: '<S6>/Saturation' incorporates:
     *  Sum: '<S2>/Sum3'
     */
    u = rtb_edPitch + rtb_Saturation_a;

    /* Bias: '<S6>/Bias' incorporates:
     *  Gain: '<S6>/Converter'
     *  Saturate: '<S6>/Saturation'
     */
    Auto_B.Bias = (u >= Auto_P.Saturation_UpperSat_h ?
                   Auto_P.Saturation_UpperSat_h : u <=
                   Auto_P.Saturation_LowerSat_g ? Auto_P.Saturation_LowerSat_g :
                   u) * Auto_P.Converter_Gain + Auto_P.Bias_Bias_j;

    /* Sum: '<S2>/Sum2' */
    rtb_Saturation_a -= rtb_edPitch;

    /* Bias: '<S7>/Bias' incorporates:
     *  Gain: '<S7>/Converter'
     *  Saturate: '<S7>/Saturation'
     */
    Auto_B.Bias_f = (rtb_Saturation_a >= Auto_P.Saturation_UpperSat_pz ?
                     Auto_P.Saturation_UpperSat_pz : rtb_Saturation_a <=
                     Auto_P.Saturation_LowerSat_l ? Auto_P.Saturation_LowerSat_l
                     : rtb_Saturation_a) * Auto_P.Converter_Gain_f +
      Auto_P.Bias_Bias_i;

    /* Sum: '<S9>/Sum2' incorporates:
     *  Gain: '<S9>/JoyScale'
     *  Inport: '<Root>/Pitch'
     *  Inport: '<Root>/RPY'
     */
    rtb_Saturation_a = Auto_P.JoyScale_Gain_d * Auto_U.Pitch - Auto_U.RPY[1];

    /* Gain: '<S13>/Filter Coefficient' incorporates:
     *  DiscreteIntegrator: '<S13>/Filter'
     *  Gain: '<S13>/Derivative Gain'
     *  Sum: '<S13>/SumD'
     */
    rtb_FilterCoefficient_j = (Auto_P.DerivativeGain_Gain_g * rtb_Saturation_a -
      Auto_DWork.Filter_DSTATE_c) * Auto_P.FilterCoefficient_Gain_k;

    /* Sum: '<S9>/Sum1' incorporates:
     *  DiscreteIntegrator: '<S13>/Integrator'
     *  Gain: '<S13>/Proportional Gain'
     *  Inport: '<Root>/omega'
     *  Sum: '<S13>/Sum'
     */
    rtb_edPitch = ((Auto_P.ProportionalGain_Gain_b * rtb_Saturation_a +
                    Auto_DWork.Integrator_DSTATE_a) + rtb_FilterCoefficient_j) -
      Auto_U.omega[1];

    /* Gain: '<S12>/Filter Coefficient' incorporates:
     *  DiscreteIntegrator: '<S12>/Filter'
     *  Gain: '<S12>/Derivative Gain'
     *  Sum: '<S12>/SumD'
     */
    rtb_FilterCoefficient_f = (Auto_P.DerivativeGain_Gain_h * rtb_edPitch -
      Auto_DWork.Filter_DSTATE_m) * Auto_P.FilterCoefficient_Gain_f;

    /* Saturate: '<S12>/Saturation' incorporates:
     *  DiscreteIntegrator: '<S12>/Integrator'
     *  Gain: '<S12>/Proportional Gain'
     *  Sum: '<S12>/Sum'
     */
    u = (Auto_P.ProportionalGain_Gain_j * rtb_edPitch +
         Auto_DWork.Integrator_DSTATE_j) + rtb_FilterCoefficient_f;

    /* Saturate: '<S8>/Saturation' incorporates:
     *  Saturate: '<S12>/Saturation'
     */
    u = u >= Auto_P.Saturation_UpperSat_m ? Auto_P.Saturation_UpperSat_m : u <=
      Auto_P.Saturation_LowerSat_k ? Auto_P.Saturation_LowerSat_k : u;

    /* Outport: '<Root>/Tail[1100:::1600]' incorporates:
     *  Bias: '<S8>/Bias'
     *  Gain: '<S8>/Converter'
     *  Saturate: '<S8>/Saturation'
     */
    Auto_Y.Tail11001600 = (u >= Auto_P.Saturation_UpperSat_mo ?
      Auto_P.Saturation_UpperSat_mo : u <= Auto_P.Saturation_LowerSat_c ?
      Auto_P.Saturation_LowerSat_c : u) * Auto_P.Converter_Gain_g +
      Auto_P.Bias_Bias_h;

    /* Sum: '<S10>/Sum1' incorporates:
     *  Gain: '<S10>/JoyScale'
     *  Inport: '<Root>/Yaw'
     *  Inport: '<Root>/omega'
     */
    rtb_edYaw = Auto_P.JoyScale_Gain_b * Auto_U.Yaw - Auto_U.omega[2];

    /* Gain: '<S14>/Filter Coefficient' incorporates:
     *  DiscreteIntegrator: '<S14>/Filter'
     *  Gain: '<S14>/Derivative Gain'
     *  Sum: '<S14>/SumD'
     */
    rtb_FilterCoefficient_a = (Auto_P.DerivativeGain_Gain_o * rtb_edYaw -
      Auto_DWork.Filter_DSTATE_a) * Auto_P.FilterCoefficient_Gain_m;

    /* Saturate: '<S14>/Saturation' incorporates:
     *  DiscreteIntegrator: '<S14>/Integrator'
     *  Gain: '<S14>/Proportional Gain'
     *  Sum: '<S14>/Sum'
     */
    u = (Auto_P.ProportionalGain_Gain_nw * rtb_edYaw +
         Auto_DWork.Integrator_DSTATE_m) + rtb_FilterCoefficient_a;

    /* Outport: '<Root>/TailVec[-45:::45]' incorporates:
     *  Saturate: '<S14>/Saturation'
     */
    Auto_Y.TailVec4545 = u >= Auto_P.Saturation_UpperSat_f ?
      Auto_P.Saturation_UpperSat_f : u <= Auto_P.Saturation_LowerSat_o ?
      Auto_P.Saturation_LowerSat_o : u;

    /* Update for DiscreteIntegrator: '<S18>/Filter' */
    Auto_DWork.Filter_DSTATE = Auto_P.Filter_gainval * rtb_FilterCoefficient +
      Auto_DWork.Filter_DSTATE;

    /* Update for DiscreteIntegrator: '<S19>/Integrator' incorporates:
     *  Gain: '<S19>/Integral Gain'
     */
    Auto_DWork.Integrator_DSTATE = Auto_P.IntegralGain_Gain_h * rtb_edRoll *
      Auto_P.Integrator_gainval + Auto_DWork.Integrator_DSTATE;

    /* Update for DiscreteIntegrator: '<S19>/Filter' */
    Auto_DWork.Filter_DSTATE_i = Auto_P.Filter_gainval_f *
      rtb_FilterCoefficient_i + Auto_DWork.Filter_DSTATE_i;

    /* Update for DiscreteIntegrator: '<S13>/Integrator' incorporates:
     *  Gain: '<S13>/Integral Gain'
     */
    Auto_DWork.Integrator_DSTATE_a = Auto_P.IntegralGain_Gain_n *
      rtb_Saturation_a * Auto_P.Integrator_gainval_p +
      Auto_DWork.Integrator_DSTATE_a;

    /* Update for DiscreteIntegrator: '<S13>/Filter' */
    Auto_DWork.Filter_DSTATE_c = Auto_P.Filter_gainval_fg *
      rtb_FilterCoefficient_j + Auto_DWork.Filter_DSTATE_c;

    /* Update for DiscreteIntegrator: '<S12>/Integrator' incorporates:
     *  Gain: '<S12>/Integral Gain'
     */
    Auto_DWork.Integrator_DSTATE_j = Auto_P.IntegralGain_Gain * rtb_edPitch *
      Auto_P.Integrator_gainval_i + Auto_DWork.Integrator_DSTATE_j;

    /* Update for DiscreteIntegrator: '<S12>/Filter' */
    Auto_DWork.Filter_DSTATE_m = Auto_P.Filter_gainval_k *
      rtb_FilterCoefficient_f + Auto_DWork.Filter_DSTATE_m;

    /* Update for DiscreteIntegrator: '<S14>/Filter' */
    Auto_DWork.Filter_DSTATE_a = Auto_P.Filter_gainval_c *
      rtb_FilterCoefficient_a + Auto_DWork.Filter_DSTATE_a;

    /* Update for DiscreteIntegrator: '<S14>/Integrator' incorporates:
     *  Gain: '<S14>/Integral Gain'
     */
    Auto_DWork.Integrator_DSTATE_m = Auto_P.IntegralGain_Gain_p * rtb_edYaw *
      Auto_P.Integrator_gainval_j + Auto_DWork.Integrator_DSTATE_m;
  } else {
    if (Auto_DWork.AutoController_MODE) {
      /* Disable for Outport: '<S2>/LeftOutPWM' */
      Auto_B.Bias = Auto_P.LeftOutPWM_Y0;

      /* Disable for Outport: '<S2>/RightOutPWM' */
      Auto_B.Bias_f = Auto_P.RightOutPWM_Y0;

      /* Disable for Outport: '<Root>/Tail[1100:::1600]' incorporates:
       *  Disable for Outport: '<S2>/TailOutPWM'
       */
      Auto_Y.Tail11001600 = Auto_P.TailOutPWM_Y0;

      /* Disable for Outport: '<Root>/TailVec[-45:::45]' incorporates:
       *  Disable for Outport: '<S2>/TailAngle'
       */
      Auto_Y.TailVec4545 = Auto_P.TailAngle_Y0;
      Auto_DWork.AutoController_MODE = FALSE;
    }
  }

  /* End of Outputs for SubSystem: '<S1>/AutoController' */

  /* Switch: '<S5>/Switch' */
  if (Auto_B.EnThr > Auto_P.Switch_Threshold) {
    /* Outport: '<Root>/Left[1100:::1350]' */
    Auto_Y.Left11001350 = rtb_Thr;
  } else {
    /* Outport: '<Root>/Left[1100:::1350]' */
    Auto_Y.Left11001350 = Auto_B.Bias;
  }

  /* End of Switch: '<S5>/Switch' */

  /* Switch: '<S5>/Switch1' */
  if (Auto_B.EnThr > Auto_P.Switch1_Threshold) {
    /* Outport: '<Root>/Right[1100:::1350]' */
    Auto_Y.Right11001350 = rtb_Thr;
  } else {
    /* Outport: '<Root>/Right[1100:::1350]' */
    Auto_Y.Right11001350 = Auto_B.Bias_f;
  }

  /* End of Switch: '<S5>/Switch1' */
}

/* Model initialize function */
void Auto_initialize(void)
{
  /* Registration code */

  /* initialize error status */
  rtmSetErrorStatus(Auto_M, (NULL));

  /* block I/O */
  (void) memset(((void *) &Auto_B), 0,
                sizeof(BlockIO_Auto));

  /* states (dwork) */
  (void) memset((void *)&Auto_DWork, 0,
                sizeof(D_Work_Auto));

  /* external inputs */
  (void) memset((void *)&Auto_U, 0,
                sizeof(ExternalInputs_Auto));

  /* external outputs */
  (void) memset((void *)&Auto_Y, 0,
                sizeof(ExternalOutputs_Auto));

  /* Start for Enabled SubSystem: '<S1>/AutoController' */
  /* InitializeConditions for DiscreteIntegrator: '<S18>/Filter' */
  Auto_DWork.Filter_DSTATE = Auto_P.Filter_IC;

  /* InitializeConditions for DiscreteIntegrator: '<S19>/Integrator' */
  Auto_DWork.Integrator_DSTATE = Auto_P.Integrator_IC;

  /* InitializeConditions for DiscreteIntegrator: '<S19>/Filter' */
  Auto_DWork.Filter_DSTATE_i = Auto_P.Filter_IC_g;

  /* InitializeConditions for DiscreteIntegrator: '<S13>/Integrator' */
  Auto_DWork.Integrator_DSTATE_a = Auto_P.Integrator_IC_d;

  /* InitializeConditions for DiscreteIntegrator: '<S13>/Filter' */
  Auto_DWork.Filter_DSTATE_c = Auto_P.Filter_IC_d;

  /* InitializeConditions for DiscreteIntegrator: '<S12>/Integrator' */
  Auto_DWork.Integrator_DSTATE_j = Auto_P.Integrator_IC_g;

  /* InitializeConditions for DiscreteIntegrator: '<S12>/Filter' */
  Auto_DWork.Filter_DSTATE_m = Auto_P.Filter_IC_c;

  /* InitializeConditions for DiscreteIntegrator: '<S14>/Filter' */
  Auto_DWork.Filter_DSTATE_a = Auto_P.Filter_IC_o;

  /* InitializeConditions for DiscreteIntegrator: '<S14>/Integrator' */
  Auto_DWork.Integrator_DSTATE_m = Auto_P.Integrator_IC_c;

  /* VirtualOutportStart for Outport: '<S2>/LeftOutPWM' */
  Auto_B.Bias = Auto_P.LeftOutPWM_Y0;

  /* VirtualOutportStart for Outport: '<S2>/RightOutPWM' */
  Auto_B.Bias_f = Auto_P.RightOutPWM_Y0;

  /* VirtualOutportStart for Outport: '<Root>/Tail[1100:::1600]' incorporates:
   *  VirtualOutportStart for Outport: '<S2>/TailOutPWM'
   */
  Auto_Y.Tail11001600 = Auto_P.TailOutPWM_Y0;

  /* VirtualOutportStart for Outport: '<Root>/TailVec[-45:::45]' incorporates:
   *  VirtualOutportStart for Outport: '<S2>/TailAngle'
   */
  Auto_Y.TailVec4545 = Auto_P.TailAngle_Y0;

  /* End of Start for SubSystem: '<S1>/AutoController' */

  /* InitializeConditions for Stateflow: '<S1>/Chart' */
  Auto_DWork.is_active_c1_Auto = 0U;
  Auto_DWork.is_c1_Auto = 0U;
  Auto_B.Auto_d = FALSE;
  Auto_B.EnThr = 0.0;
}

/* Model terminate function */
void Auto_terminate(void)
{
  /* (no terminate code required) */
}

/*
 * File trailer for generated code.
 *
 * [EOF]
 */
